typedef enum
{
  rightdump,
  rightnodump,
  leftdump,
  leftnodump
} TStates;
TStates automode;


typedef enum
{
  only3,
  only4,
  both,
  neither
} TStates2;
TStates2 TSmode;

int btn=7;


string mode;

#pragma systemFile
#include "drivers\HTIRS2-driver.h"
#include "drivers\LEGOTS-driver.h"
#include "drivers\LEGOUS-driver.h"
#include "joystickdriver.c"
#include "gyro.h"
const bool HAS_GYRO = true;
const bool HAS_ACCEL = false;
const tSensors gyroPort = S4;
const tSensors accelPort = S2;


void left(int speed)
{
  motor[frontl]=speed;
  motor[backl]=speed;
  motor[frontr]=-speed;
  motor[backr]=-speed;
  return;
}
void leftSide(int speed)
{
  motor[frontl]=speed;
  motor[backl]=speed;
  return;
}
void rightSide(int speed)
{

  motor[frontr]=speed;
  motor[backr]=speed;
  return;
}
void right(int speed)
{
  motor[frontl]=-speed;
  motor[backl]=-speed;
  motor[frontr]=speed;
  motor[backr]=speed;
  return;
}
void turn(int speed1, int speed2)
{
  motor[frontl]=speed1;
  motor[backl]=speed1;
  motor[frontr]=speed2;
  motor[backr]=speed2;
  return;
}
void straight(int speed, int offset=0, int gyros=1)
{
  if (gyros==1){
  diff=gyroGetHeading()-offset;}
  else{diff=0;}
turn(speed+diff,speed-diff);
  return;
}
void stop()
{
  motor[backr]=0;
    motor[frontr]=0;
    motor[backl]=0;
    motor[frontl]=0;}

void straightdist(int speed,int encoder, int offset=0, int gyros=1)
{
  nMotorEncoder[frontl]=0;
 nMotorEncoder[frontr]=0;
 nMotorEncoder[backl]=0;
 nMotorEncoder[backr]=0;
  while (abs(nMotorEncoder[frontr])<encoder||abs(nMotorEncoder[frontl])<-encoder||abs(nMotorEncoder[backr])<encoder||abs(nMotorEncoder[backl])<-encoder) {

 if (gyros==1){
  diff=gyroGetHeading()-offset;}
  else{diff=0;}
turn(speed-diff,speed+diff);
turn(speed-diff,speed+diff);}  return;
}

float currDir;
float IRGetACDir(tMUXSensor link)
{
    static float prevDir = 0.0;

    int acS[5];
    int idx;

    idx = HTIRS2readACDir(link);
    currDir = (float)idx;
    if (idx == 0)
    {
        currDir = prevDir;
    }
    else if (HTIRS2readAllACStrength(link, acS[0], acS[1], acS[2], acS[3], acS[4]))
    {
        idx = (idx - 1)/2;
        if ((idx < 4) && (acS[idx] != 0) && (acS[idx + 1] != 0))
        {
            currDir += (float)(acS[idx + 1] - acS[idx])/
                          max(acS[idx], acS[idx + 1]);
        }
        nxtDisplayTextLine(0, "Idx=%d,Dir=%5.1f", idx, currDir);
        nxtDisplayTextLine(2, "S1=%d,S2=%d", acS[0], acS[1]);
        nxtDisplayTextLine(3, "S3=%d,S4=%d", acS[2], acS[3]);
        nxtDisplayTextLine(4, "S5=%d", acS[4]);
    }
    prevDir = currDir;

    return currDir;
}
void rightDump()
{servo[cup2]=255;
  nMotorEncoder[frontl]=0;
 nMotorEncoder[frontr]=0;
 nMotorEncoder[backl]=0;
 nMotorEncoder[backr]=0;
 straight(25);


       straightdist(40,2950);


straight(0);
wait10Msec(50);
servo[d2]=200;
wait10Msec(100);

}
void close(){
servo[d2]=0;
servo[cup2]=0;
servo[d1]=255;
servo[cup1]=255;}
void open(){
servo[d2]=255;
servo[cup2]=255;
servo[d1]=0;
servo[cup1]=0;}
void leftDump()
{ servo[cup1]=0;
  nMotorEncoder[frontl]=0;
 nMotorEncoder[frontr]=0;
 nMotorEncoder[backl]=0;
 nMotorEncoder[backr]=0;
       straightdist(40,2950,0);

straight(0);
wait10Msec(50);
servo[d1]=100;
wait10Msec(100);

 }
 void noDump()
{
       straightdist(40,2950);


straight(0);
}
 int wait=0;
void initializeRobot()
{


 servo[d1]=255;
servo[cup1]=255;
servo[d2]=0;
servo[cup2]=0;
 servo[rg1]=128;
      servo[rg2]=128;
         servo[lg1]=165;
      servo[lg2]=90;


      automode= rightdump;
       mode="right w/ dump";

        nNxtExitClicks=3;

while(btn!=0)
{nxtDisplayCenteredTextLine(6,"%s",mode);
nxtDisplayCenteredTextLine(7,"%i",wait);
btn=nNxtButtonPressed;
if (btn==3)
{wait=wait+1;}
if (btn==1)
{switch(automode){
  case rightdump:
  automode=rightnodump;
  mode="right w/o dump";

  break;

  case rightnodump:
  automode=rightdump;
  mode="right w/ dump";

  break;

  case leftdump:
  automode=leftnodump;
  mode="left w/o dump";

  break;

  case leftnodump:
  automode=leftdump;
  mode="left w/ dump";

  break;
}}
if (btn==2)
{switch(automode){
  case rightdump:
  automode=leftdump;
  mode="left w/ dump";

  break;

  case rightnodump:
  automode=leftnodump;
  mode="left w/o dump";

  break;

  case leftdump:
  automode=rightdump;
  mode="right w/ dump";

  break;

  case leftnodump:
  automode=rightnodump;
  mode="right w/o dump";

  break;
}}


wait10Msec(10);
}
 HTSMUXinit();
 HTSMUXscanPorts(mux);
HTSMUXscanPorts(IRmux);
nxtDisplayBigTextLine(6,"Calibrating...");  float sum=0;
  float i;
  for (i=1;i<=500;i++)
  {
    sum=SensorValue(gyro)+sum;
		wait1Msec(4);
  }
  gyroZeroBias=sum/500;
gyroSetDegrees(0);
StartTask(gyroUpdate);
nxtDisplayBigTextLine(6,"Done");

  return;
}
